import ctypes
import logging
import os
import sys
import time
from socket import socket
import logging
import serial

logging.basicConfig(filename='xuanzhuan.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')


class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


last_set = [[[-81.361, 15.942, 69.065, 0.796, 93.169, -81.208], [-0.046409, 0.283399, 0.264959, -3.111, 0.018, -0.001],
             [-0.04463, 0.280579, 0.36101, -3.111, 0.018, -0.001]],
            [[-60.854, 11.685, 70.887, 0.104, 95.439, -60.765], [-0.131259, 0.234479, 0.267119, -3.111, 0.017, -0.001],
             [-0.12933, 0.231389, 0.374429, -3.111, 0.018, -0.001]],
            [[-39.943, 14.611, 70.338, -0.671, 92.801, -40.004], [-0.21516, 0.18397, 0.262279, -3.108, 0.02, 0.0],
             [-0.21313, 0.180679, 0.36282, -3.107, 0.023, 0.001]],
            [[-23.104, 26.405, 45.615, -1.355, 105.853, -23.52], [-0.292049, 0.12816, 0.39075, -3.105, 0.025, 0.001],
             [-0.292049, 0.12816, 0.39075, -3.105, 0.025, 0.001]]
            ]


class SerCom():
    """
    串口建立，发送 接受
    """

    def __init__(self, path, baud_rate):

        # 串口设备的名称，例如 /dev/ttyUSB0（Linux 系统）或 COM1（Windows 系统）
        # 波特率，即数据传输速率

        try:
            # 打开串口
            self.ser = serial.Serial(path, baud_rate, timeout=1, parity=serial.PARITY_NONE, bytesize=serial.EIGHTBITS,
                                     stopbits=serial.STOPBITS_ONE)
        except serial.SerialException as e:
            # 捕获异常并打印错误原因
            logging.info('串口连接失败！原因为：', e)
        else:
            # 连接成功，打印串口信息
            print('串口连接成功！串口信息：', self.ser)

    def is_connect(self):
        """
        判断ser是否连接成功
        """

        if self.ser.is_open:
            print('串口连接成功！')
            logging.info('串口建立连接')
        else:
            logging.info('串口连接失败')

    def send(self):

        data = bytes.fromhex('01 06 60 02 00 10 37 C6')
        # 发送数据
        self.ser.write(data)

    def read(self):

        ret = self.ser.read()

    def close(self):

        self.ser.close()


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')

    def arm_transport(self, i):
        """
        运输物料盘：
        :return:
        """

        #    初始化
        tag = self.pDll.Set_Tool_Voltage(self.nSocket, 3, 1)
        logging.info(f'设置末端电压：{tag}')

        tag = self.pDll.Set_Gripper_Release(self.nSocket, 750, 1)
        logging.info(f'夹爪张开：{tag}')

        ser.send()
        logging.info('旋转仓储旋转90°')

        self.pDll.Set_Lift_Height.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_bool)
        self.pDll.Set_Lift_Height.restype = ctypes.c_int
        tag = self.pDll.Set_Lift_Height(self.nSocket, 230, 50, 1)
        logging.info(f'导轨初始位置：{tag}')

        float_joint = ctypes.c_float * 6
        joint1 = float_joint(*[131.682, -4.102, -63.948, -0.335, -110.513, -51.525])
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte, ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movej_Cmd(self.nSocket, joint1, 20, 0, 1)
        logging.info(f'运行至初始位置：{tag}')

        #   运输物料盘
        joint2 = float_joint(*[96.009, -40.432, -38.951, 0.515, -101.218, -82.26])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint2, 20, 0, 1)
        logging.info(f'运行至物料盘上方：{tag}')

        po1 = POSE()
        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = [-0.040369, 0.368299, 0.29172, -3.13, -0.007, 3.112]
        self.pDll.Movel_Cmd.argtypes = (ctypes.c_int, POSE, ctypes.c_byte, ctypes.c_float, ctypes.c_int)
        self.pDll.Movel_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'准备夹取：{tag}')

        tag = self.pDll.Set_Gripper_Pick(self.nSocket, 750, 750, 1)
        time.sleep(1)
        logging.info(f'夹取物料盘:{tag}')

        po1.pz += 0.05
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'离开仓储平台:{tag}')

        joint2 = float_joint(*[132.565, -16.04, -49.221, 1.529, -117.154, -46.861])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint2, 20, 0, 1)
        logging.info(f'机械臂初始位置：{tag}')

        tag = self.pDll.Set_Lift_Height(self.nSocket, 434, 20, 1)  # 平面仓储放置物料盘
        logging.info(f'导轨移动到固定位置:{tag}')


        #   放置位置上方

        joint3 = float_joint(*last_set[i][0])
        tag = self.pDll.Movej_Cmd(self.nSocket, joint3, 20, 0, 1)
        logging.info(f'机械臂移动到放置位置上方:{tag}')

        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = last_set[i][1]
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'放置位置:{tag}')

        tag = self.pDll.Set_Gripper_Release(self.nSocket, 500, 1)
        logging.info(f'放置成功：{tag}')

        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = last_set[i][2]
        tag = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'退回放置点上方:{tag}')

    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)


def main(ROT2, i):
    ret = ROT2.arm_transport(ser, i)
    logging.info(f'物料盘运输成功:{ret}')


if __name__ == '__main__':
    ROT2 = Arm(ip="192.168.1.18")
    ser = SerCom('COM1', 38400)
    for i in range(4):
        main(ROT2, i)
    ROT2.close()
    ser.close()
